package es.uji.viselab.visualservoing;

import javax.swing.JPanel;
import javax.vecmath.Vector2d;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.camera.CameraCatalog;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.model.ModelCreator;

public class SimplePBVS extends VisualServoingTask {

	public SimplePBVS(ExperimentParameters ep) {
		super(ep);
	}

	@Override
	public double[] compute(ImageSource image) {
		
		CameraCatalog camera = ep.getCamParameters();
		double df = camera.getFocalDistanceCorrected();
		
		Vector2[] imageCoordinateList = image.getImageCoordinateList();
		 Vector3[] objectCoordinateList = ModelCreator.get3d(ep.getModel(),ep.getCamParameters(),ep.getTarget());
			
		double perimeterObject = 0;
		
		for (int i = 0; i < 3; i++) {
			perimeterObject = perimeterObject
					+ distance(objectCoordinateList[i].x, objectCoordinateList[i].y,
							objectCoordinateList[i + 1].x, objectCoordinateList[i + 1].y);
		}
		perimeterObject = perimeterObject
				+ distance(objectCoordinateList[3].x, objectCoordinateList[3].y,
						objectCoordinateList[0].x, objectCoordinateList[0].y);
		perimeterObject=perimeterObject/4;
		
		double perimeterImage = 0;
		for (int i = 0; i < 3; i++) {
			perimeterImage = perimeterImage
					+ distance(imageCoordinateList[i].x, imageCoordinateList[i].y,
							imageCoordinateList[i + 1].x, imageCoordinateList[i + 1].y);
		}
		perimeterImage = perimeterImage
				+ distance(imageCoordinateList[3].x, imageCoordinateList[3].y,
						imageCoordinateList[0].x, imageCoordinateList[0].y);
		perimeterImage=perimeterImage/4;
		
		double distance = perimeterObject * df / perimeterImage;
		
		double[] velocities = {0,0,0,0,0,0};
		velocities[2]=distance;
		return velocities;
        
	}

	

	private double distance(double x1, double y1, double x2, double y2) {
		double vectorX = x2 - x1;
		double vectorY = y2 - y1;
		return Math.sqrt(Math.pow(vectorX, 2) + Math.pow(vectorY, 2));
	}

	

	@Override
	public JPanel getPanel() {
		return null;
	}

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		//compute(i);
	}

	@Override
	public double getError() {
		return error;
	}

}
